#pragma once

#include "byte_tracker/BYTETracker.h"
#include "math/polygon2d.h"

#include <rclcpp/rclcpp.hpp>

namespace perception::camera {

static inline double legalize_angle(double a)
{
    while (a < 0) {
        a += 360;
    }
    while (a >= 360) {
        a -= 360;
    }
    return a;
}

static inline double in_angle(double hea1, double hea2)
{
    hea1 = legalize_angle(hea1);
    hea2 = legalize_angle(hea2);

    double retv = std::fabs(hea2 - hea1);
    retv = (retv > 180.0) ? (360.0 - retv) : retv;

    return retv;
}

#define ANGLE_DIFF(diff, x2, x1)     \
    do {                             \
        diff = x2 - x1 + 180;        \
        if (diff < 0) {              \
            diff = diff + 360 - 180; \
        } else if (diff > 360) {     \
            diff = diff - 360 - 180; \
        } else {                     \
            diff = diff - 180;       \
        }                            \
    } while (0)

static inline double angle_average(const std::list<double>& angles)
{
    std::vector<double> angle(angles.begin(), angles.end());

    double diff = 0;
    double last = angle[0];
    double sum = angle[0];

    uint32_t i = 0;
    for (i = 1; i < angle.size(); i++) {
        ANGLE_DIFF(diff, angle[i], angle[i - 1]);
        last += diff;
        sum += last;
    }

    return legalize_angle(sum / angle.size());
}

struct OneObject {
    void speed(int64_t ts_ms, double& vx, double& vy)
    {
        if (distances_.size() <= 1) {
            vx = vy = NAN;
            return;
        }

        auto iter = distances_.upper_bound(ts_ms - 1.0 * 1000); // 2s之前
        if (iter != distances_.end()) {
            auto time_diff = distances_.rbegin()->first - iter->first;
            vx = (distances_.rbegin()->second.first - iter->second.first) / time_diff * 1000;
            vy = (distances_.rbegin()->second.second - iter->second.second) / time_diff * 1000;
            return;
        }

        auto time_diff = distances_.rbegin()->first - distances_.begin()->first;
        vx = (distances_.rbegin()->second.first - distances_.begin()->second.first) / time_diff * 1000;
        vy = (distances_.rbegin()->second.second - distances_.begin()->second.second) / time_diff * 1000;
    }

    void stable_position(int64_t ts_ms, double& x, double& y)
    {
        if (distances_.empty()) {
            return;
        }

        auto iter = distances_.upper_bound(ts_ms - 0.8 * 1000); // 0.8s之前
        if (iter == distances_.end()) {
            iter = distances_.begin();
        }

        double acc_x = 0, acc_y = 0;
        size_t counter = 0;
        for (; iter != distances_.end(); iter++) {
            acc_x = iter->second.first;
            acc_y = iter->second.second;
            counter++;
        }
        x = acc_x / counter;
        y = acc_y / counter;
    }

    void add_pos(int64_t ms, double x, double y)
    {
        updated_time_ms_ = ms;
        distances_[ms] = { x, y };
    }

    double heading()
    {
        const double aver = angle_average(hea_cache_);
        std::list<double> heas;
        for (auto& hea : hea_cache_) {
            if (in_angle(aver, hea) > 15) {
                continue;
            }
            heas.push_back(hea);
        }
        return heas.empty() ? aver : angle_average(heas);
    }

    double heading2()
    {
        const double aver = angle_average(hea_cache_);
        double min_diff_hea = *(hea_cache_.begin());
        for (auto& hea : hea_cache_) {
            const double diff = in_angle(aver, hea);
            if (diff < min_diff_hea) {
                min_diff_hea = diff;
            }
        }
        return min_diff_hea;
    }

    double heading3()
    {
        if (distances_.size() <= 1) {
            return NAN;
        }

        auto iter = distances_.upper_bound(distances_.rbegin()->first - 2.0 * 1000); // 2s之前
        if (iter == distances_.end()) {
            iter = distances_.begin();
        }

        decision::math::Segment2d segment(decision::math::Vec2d(iter->second.first, iter->second.second), 
            decision::math::Vec2d(distances_.rbegin()->second.first, distances_.rbegin()->second.second));

        if(segment.length() < 2) {
            return NAN;
        }

        return legalize_angle((M_PI_2 - segment.heading()) / M_PI * 180);
    }

    void add_heading(double hea)
    {
        hea_cache_.push_back(hea);
        while (hea_cache_.size() > hea_cache_num_) {
            hea_cache_.pop_front();
        }
    }

    size_t headings() {
        return hea_cache_.size();
    }

    int64_t updated_time_ms_;
    std::map<int64_t, std::pair<double, double>> distances_;

    std::list<double> hea_cache_;
    double last_hea_cache_ = NAN;
    size_t hea_cache_num_ = 16;
};

class OneCameraTracker {
public:
    OneCameraTracker(std::string inex_, int pw, int ph, int rw, int rh, std::string roi, std::string camera);

    void process(const std::vector<ObjectDetectInfoPtr>& objects, std::vector<ObjectTrackInfoPtr>& tracked_objects,
        const cv::Mat& img, int64_t ts_ms);

    void convert_uv2xy(const std::string& hit, const double u, const double v, double& x, double& y);

    bool filter_out(const cv::Mat& roi, int x, int y, int w, int h);

    void read_3d_roi();
    void read_2d_roi();

private:
    const std::string inex_;
    int p_image_width_;
    int p_image_height_;
    int r_image_width_;
    int r_image_height_;
    const std::string roi_file_;
    const std::string camera_;

    std::shared_ptr<BYTETracker> tracker_;

    decision::math::Polygon2d roi_3d_;
    bool valid_roi_3d_ = false;

    cv::Mat roi_2d_;
    bool valid_roi_2d_ = false;

    rclcpp::Logger logger_;

    cv::Matx<double, 3, 3> camera_matrix_;
    cv::Matx<double, 1, 5> dist_coeffs_;
    cv::Matx<double, 3, 3> cam2ground_;

    std::map<int, OneObject> speed_calcer_;
};

}
